/*
   FieldVis - A Tool for Visualizing Astrophysical Magnetohydrodynamic Flow
   
   Copyright (C) 2006 - 2008     FieldVis Development Team
  
   This program is free software; you can redistribute it and/or modify
   it under the terms of the GNU General Public License as published by
   the Free Software Foundation; either version 2 of the License, or
   (at your option) any later version.
  
   This program is distributed in the hope that it will be useful,
   but WITHOUT ANY WARRANTY; without even the implied warranty of
   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
   GNU General Public License for more details.
  
   You should have received a copy of the GNU General Public License along
   with this program; if not, write to the Free Software Foundation, Inc.,
   51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.

   File        : Matrix4x4.cpp
   Author      : Jeremy Moc  (mocj@msi.umn.edu)

   Description : 
*/

#include <cmath>

#include "Matrix4x4.h"

double GetLinePlaneIntersectDist(Vector3D  &TestPt, Vector3D const &RayVect,
                                 Vector3D const &PlanePt, Vector3D &PlaneVect)
{
   // returns the distance from the point to the plane along the given ray
   // assumes vectors are normalized

   double PlaneDot, TestDot, PlanarDist, RayPlaneDot;
   Vector3D MapTestPt, Test2Plane;

   // calculate distance from plane to test point perpendicular to plane
   PlaneDot = PlanePt.Dot(PlaneVect);
   TestDot = TestPt.Dot(PlaneVect);
   PlanarDist = PlaneDot - TestDot;

   // calculate dot product ray and plane vectors (i.e. how perpendicular ray is to plane)
   RayPlaneDot = RayVect.Dot(PlaneVect);

   // ray is parallel with plane, intersection at infinity
   //if (RayPlaneDot == 0.0) return INFINITY;
   if (RayPlaneDot == 0.0) return 100000000.0;
   // calculate distance with respect to ray direction (positive if in front of
   //   test point, negative if behind)
   else
   {
      // map ray point to plane
      MapTestPt = TestPt + (PlaneVect * PlanarDist);

      // calculate vector from test point to mapped point on plane
      Test2Plane = MapTestPt - TestPt;

      // return distance by scaling direct planar distance by dot product
      return PlanarDist / RayPlaneDot;
   }
}


Matrix4x4 CalcRotMat(Vector3D const &Axis, double Angle)
{
   // returns the 4x4 rotation matrix for the given axis and angle
   // assumes axis is normalized

   Matrix4x4 M;
   double SinTheta, CosTheta;

   // calculate trig functions
   SinTheta = sin(Angle);
   CosTheta = cos(Angle);

   // calculate the matrix
   M.Elems[0] = CosTheta + Axis.GetX() * Axis.GetX() * (1.0 - CosTheta);
   M.Elems[1] = Axis.GetX() * Axis.GetY() * (1.0 - CosTheta) + Axis.GetZ() * SinTheta;
   M.Elems[2] = Axis.GetX() * Axis.GetZ() * (1.0 - CosTheta) - Axis.GetY() * SinTheta;
   M.Elems[3] = 0.0;
   M.Elems[4] = Axis.GetX() * Axis.GetY() * (1.0 - CosTheta) - Axis.GetZ() * SinTheta;
   M.Elems[5] = CosTheta + Axis.GetY() * Axis.GetY() * (1.0 - CosTheta);
   M.Elems[6] = Axis.GetY() * Axis.GetZ() * (1.0 - CosTheta) + Axis.GetX() * SinTheta;
   M.Elems[7] = 0.0;
   M.Elems[8] = Axis.GetX() * Axis.GetZ() * (1.0 - CosTheta) + Axis.GetY() * SinTheta;
   M.Elems[9] = Axis.GetY() * Axis.GetZ() * (1.0 - CosTheta) - Axis.GetX() * SinTheta;
   M.Elems[10] = CosTheta + Axis.GetZ() * Axis.GetZ() * (1.0 - CosTheta);
   M.Elems[11] = 0.0;
   M.Elems[12] = 0.0;
   M.Elems[13] = 0.0;
   M.Elems[14] = 0.0;
   M.Elems[15] = 1.0;

   return M;
}


Vector3D TransformPoint(Vector3D const &P, Matrix4x4 const &M)
{
   // applies complete transformation (rotation plus translation) defined by
   //   matrix to point

   Vector3D TransP;

   // calculate the transformed coordinates
   TransP.SetX(P.GetX() * M.Elems[0] + P.GetY() * M.Elems[4] + P.GetZ() * M.Elems[8] + M.Elems[12]);
   TransP.SetY(P.GetX() * M.Elems[1] + P.GetY() * M.Elems[5] + P.GetZ() * M.Elems[9] + M.Elems[13]);
   TransP.SetZ(P.GetX() * M.Elems[2] + P.GetY() * M.Elems[6] + P.GetZ() * M.Elems[10] + M.Elems[14]);

   return TransP;
}


Vector3D TransformVector(Vector3D const &V, Matrix4x4 const &M)
{
   // applies only rotation component of transformation defined by matrix to
   //   vector

   Vector3D TransV;

   // calculate the transformed coordinates
   TransV.SetX(V.GetX() * M.Elems[0] + V.GetY() * M.Elems[4] + V.GetZ() * M.Elems[8]);
   TransV.SetY(V.GetX() * M.Elems[1] + V.GetY() * M.Elems[5] + V.GetZ() * M.Elems[9]);
   TransV.SetZ(V.GetX() * M.Elems[2] + V.GetY() * M.Elems[6] + V.GetZ() * M.Elems[10]);

   return TransV;
}

